#ifndef KALMAN_FILTER_HPP
#define KALMAN_FILTER_HPP

class KalmanFilter 
{
    public:
        // 1维卡尔曼滤波器状态结构体
        typedef struct {
            float x;        /* 状态值 */
            float A;        /* 状态转移矩阵 */
            float H;        /* 测量矩阵 */
            float q;        /* 过程噪声协方差 */
            float r;        /* 测量噪声协方差 */
            float p;        /* 估计误差协方差 */
            float gain;     /* 卡尔曼增益 */
        } Kalman1State;

        // 2维卡尔曼滤波器状态结构体
        typedef struct {
            float x[2];     /* 状态数组：[0]为角度，[1]为角度变化率 */
            float A[2][2];  /* 状态转移矩阵（2x2） */
            float H[2];     /* 测量矩阵（1x2） */
            float q[2];     /* 过程噪声协方差（2x1） */
            float r;        /* 测量噪声协方差 */
            float p[2][2];  /* 估计误差协方差（2x2） */
            float gain[2];  /* 卡尔曼增益（2x1） */
        } Kalman2State;

        /* 1维卡尔曼滤波器初始化
        * @param state：1维滤波器状态结构体指针
        * @param init_x：初始状态值
        * @param init_p：初始估计误差协方差
        */
        static void kalman1Init(Kalman1State* state, float init_x, float init_p);

        /* 1维卡尔曼滤波计算
        * @param state：1维滤波器状态结构体指针
        * @param z_measure：测量值
        * @return：滤波后的估计值
        */
        static float kalman1Filter(Kalman1State* state, float z_measure);

        /* 2维卡尔曼滤波器初始化
        * @param state：2维滤波器状态结构体指针
        * @param init_x：初始状态数组（长度为2）
        * @param init_p：初始估计误差协方差矩阵（2x2）
        */
        static void kalman2Init(Kalman2State* state, float* init_x, float (*init_p)[2]);

        /* 2维卡尔曼滤波计算
        * @param state：2维滤波器状态结构体指针
        * @param z_measure：测量值
        * @return：滤波后的主要状态值（对应x[0]）
        */
        static float kalman2Filter(Kalman2State* state, float z_measure);
};

#endif  // KALMAN_FILTER_HPP